/*
 * imu_sensors.c
 *
 * Created: 8/14/2011 11:03:31 PM
 *  Author: GrubyGrub
 */ 

#include <asf.h>
#include "imu_sensors.h"
#include "module_init.h"
#include "conf_pdca.h"
#include "conf_startup.h"




/*Init sensors, it will setup all the MARG sensors for data collection, data collection is not done in this step.
TODO: handle edge cases 'broken' sensors*/
void init_sensors(struct marg_sensors_t* marg_s)
{
	 switch (marg_s->sensor_t)
	 {
		 case ACCELEROMETER:
		 
			  marg_s->A_Lock = false;
			  marg_s->A_Set = 0;
			  
			  /*ACC Bootup sequence*/
	 
			 /*addr of chip to read*/
			 (marg_s->Acc_pkt).chip = ACC_ADDRESS;
	 
			 /*addr of internal read/write, internal register that is r/w*/
			 (marg_s->Acc_pkt).addr = CTRL_REG4_A; 
	
			 /*length of internal register, 1 byte in this case*/
			 (marg_s->Acc_pkt).addr_length = INTERNAL_ADDR_LEN;
	 
			 /*where our input data is stored*/
			 (marg_s->Acc_pkt).buffer = (marg_s->Acc_buf);
	 
			 /*(marg_s->Acc_pkt) long is our out data, 1 byte*/
			 (marg_s->Acc_pkt).length = SETTINGS_DATA_LENGTH;
	 
			 /*select 2g as acc scale, contouous update msb/lsb while reading*/
			 (marg_s->Acc_buf)[0] = 0b00000000;
	 
			 while(twi_master_write(&AVR32_TWI,&(marg_s->Acc_pkt)) != TWI_SUCCESS);
	 
			 /*select reg*/
			 (marg_s->Acc_pkt).addr = CTRL_REG1_A;
	 
	 		 /*power on, 1khz sample rate*/
			 (marg_s->Acc_buf)[0] = 0b00111111;
			 
			 while(twi_master_write(&AVR32_TWI,&(marg_s->Acc_pkt)) != TWI_SUCCESS);
			 
			 /*set reading options*/			 
			 /*chip addr*/
			 (marg_s->Acc_pkt_r).chip = ACC_ADDRESS;
	 
			 /*internal addr*/
			 (marg_s->Acc_pkt_r).addr_length = INTERNAL_ADDR_LEN;
	 
			 /*slave subaddressing so the address will increase automatically next time it is read*/
			 (marg_s->Acc_pkt_r).addr = OUT_X_L_A | (1 << 7);
	 
			 (marg_s->Acc_pkt_r).buffer = marg_s->Acc_buf;
	 
			 (marg_s->Acc_pkt_r).length = READ_LENGTH;
		 
		 break;
		 
		 case MAGNETOMETER:
		 
			 marg_s->M_Lock = false;
			 marg_s->M_Set = 0;
		 
			 /*mag boot up*/
			 /*addr of chip to read*/
			 (marg_s->Mag_pkt).chip = MAG_ADDRESS;
	 
			 /*addr of internal read/write, internal register that is r/w*/
			 (marg_s->Mag_pkt).addr = CRA_REG_M; /*power and speed settings reg*/
	
			 /*length of internal resgiter, 1 byte in this case*/
			 (marg_s->Mag_pkt).addr_length = INTERNAL_ADDR_LEN;
	 
			 /*where our input data is stored*/
			 (marg_s->Mag_pkt).buffer = (marg_s->Mag_buf);
	 
			 /*(marg_s->Acc_pkt) long is our out data, 1 byte*/
			 (marg_s->Mag_pkt).length = SETTINGS_DATA_LENGTH;
	 
			 /*75 hz refresh rate*/
			 (marg_s->Mag_buf)[0] = 0b00011000;
			 
			 twi_master_write(&AVR32_TWI,&(marg_s->Mag_pkt));
			 
			 /*1.3 gauss sensitivity*/
			 (marg_s->Mag_buf)[0] = 0b00100000;
			 (marg_s->Mag_pkt).addr = CRB_REG_M;
			 
			  while(twi_master_write(&AVR32_TWI,&(marg_s->Mag_pkt)) != TWI_SUCCESS);
			 
			 
			  /*power on, contious sample*/
			 (marg_s->Mag_buf)[0] = 0b00000000;
			 (marg_s->Mag_pkt).addr = MR_REG_M;
			 
			  while(twi_master_write(&AVR32_TWI,&(marg_s->Mag_pkt)) != TWI_SUCCESS);	
						  
						  
			 /*prepare for read*/
			 /*chip addr*/
			 (marg_s->Mag_pkt_r).chip = MAG_ADDRESS;
	 
			 /*internal addr*/
			 (marg_s->Mag_pkt_r).addr_length = INTERNAL_ADDR_LEN;
	 
			 /*slave subaddressing so the address will increase automatically next time it is read*/
			 (marg_s->Mag_pkt_r).addr = OUT_X_H_M | (1 << 7);
	 
			 (marg_s->Mag_pkt_r).buffer = marg_s->Mag_buf;
	 
			 (marg_s->Mag_pkt_r).length = READ_LENGTH;  
			 
		 break;
		 
		 case GYROSCOPE:
		 
			 marg_s->G_Lock = false;
			 marg_s->G_Set = 0;
			 
			 /*gyroscope boot*/
			 (marg_s->Gyro_pkt).chip = GYRO_ADDRESS;
			 
			  /*length of internal resgiter, 1 byte in this case*/
			 (marg_s->Gyro_pkt).addr_length = INTERNAL_ADDR_LEN;
	 
			 /*where our input data is stored*/
			 (marg_s->Gyro_pkt).buffer = (marg_s->Gyro_buf);
	 
			 /*(marg_s->Gyro_pkt) long is our out data, 1 byte*/
			 (marg_s->Gyro_pkt).length = SETTINGS_DATA_LENGTH;
			 
			 /*addr of internal read/write, internal register that is r/w*/
			 (marg_s->Gyro_pkt).addr = CTRL_REG4_G;
			 
		     /*Contiuously update buffer while reading, sensitivity: 250DPS, can change to 500 DPS/2000
			 0b00[DPS - 00, 01, 10]0000*/
			 /*currently at 500 dps*/
			 (marg_s->Gyro_buf)[0] = 0b00010000;
			 
			 while(twi_master_write(&AVR32_TWI,&(marg_s->Gyro_pkt)) != TWI_SUCCESS);	 
			 
			 	 	 
			 (marg_s->Gyro_pkt).addr = CTRL_REG1_G;
			 
			  /*800hz ODR, cut-off 30 hz, power on*/
			 (marg_s->Gyro_buf)[0] = 0b11111111;	
			 while(twi_master_write(&AVR32_TWI,&(marg_s->Gyro_pkt)) != TWI_SUCCESS);	 
		 
			/*set reading options*/
			 /*chip addr*/
			 (marg_s->Gyro_pkt_r).chip = GYRO_ADDRESS;
	 
			 /*internal addr*/
			 (marg_s->Gyro_pkt_r).addr_length = INTERNAL_ADDR_LEN;
	 
			 /*slave subaddressing so the address will increase automatically next time it is read*/
			 (marg_s->Gyro_pkt_r).addr = OUT_X_L_G | (1 << 7);
	 
			 (marg_s->Gyro_pkt_r).buffer = marg_s->Gyro_buf;
	 
			 (marg_s->Gyro_pkt_r).length = READ_LENGTH;
			 		 
			
		 break;
		 
		 case ITG3200:
		 
			 marg_s->G_Lock = false;
			 marg_s->G_Set = 0;
			 			 
			 /*gyroscope boot*/
			 (marg_s->Gyro_pkt).chip = ITG_ADDRESS;
			 
			  /*length of internal resgiter, 1 byte in this case*/
			 (marg_s->Gyro_pkt).addr_length = INTERNAL_ADDR_LEN;
	 
			 /*where our input data is stored*/
			 (marg_s->Gyro_pkt).buffer = (marg_s->Gyro_buf);
	 
			 /*(marg_s->Gyro_pkt) long is our out data, 1 byte*/
			 (marg_s->Gyro_pkt).length = SETTINGS_DATA_LENGTH;
			 
			 (marg_s->Gyro_pkt).addr = PWR_MGM_ITG;
			 (marg_s->Gyro_buf)[0] = 0x80;	 
			 
			 
			printf("probe: %d\n", twi_probe(&AVR32_TWI,ITG_ADDRESS));
			 while(twi_master_write(&AVR32_TWI,&(marg_s->Gyro_pkt)) != TWI_SUCCESS);	 
			 
			 	 	 
			 (marg_s->Gyro_pkt).addr = SMPLRT_DIV_ITG;
			 (marg_s->Gyro_buf)[0] = 0x00;	
			 while(twi_master_write(&AVR32_TWI,&(marg_s->Gyro_pkt)) != TWI_SUCCESS);
			 
			 (marg_s->Gyro_pkt).addr = DLPF_FS_ITG;
			 (marg_s->Gyro_buf)[0] = 0x18;	 
			 while(twi_master_write(&AVR32_TWI,&(marg_s->Gyro_pkt)) != TWI_SUCCESS);	 	 
			
			
			 (marg_s->Gyro_pkt).addr = INT_CFG_ITG;
			 (marg_s->Gyro_buf)[0] = 0x05;	 
			 while(twi_master_write(&AVR32_TWI,&(marg_s->Gyro_pkt)) != TWI_SUCCESS);	 	 
	
	
			
			/*set reading options*/
			 /*chip addr*/
			 (marg_s->Gyro_pkt_r).chip = ITG_ADDRESS;
	 
			 /*internal addr*/
			 (marg_s->Gyro_pkt_r).addr_length = INTERNAL_ADDR_LEN;
	 
			 /*slave subaddressing so the address will increase automatically next time it is read*/
			 (marg_s->Gyro_pkt_r).addr = GYRO_XOUT_H_ITG | (1 << 7);
	 
			 (marg_s->Gyro_pkt_r).buffer = marg_s->Gyro_buf;
	 
			 (marg_s->Gyro_pkt_r).length = READ_LENGTH;
			 		 
			
		 break;
		 
		 
		 default:
		 break;
	 }
}

/*read sensors async - using DMA*/
void read_sensors_r(struct marg_sensors_t* marg_s)
{	 		 
	//load the channel so we can start the DMA*/	 
	
	while(true)
	{
	
		pdca_load_channel(TWI_PDCA_CHANNEL,(void *)marg_s->Acc_buf,READ_LENGTH);
		//start the DMA
		twi_master_read_dma(&AVR32_TWI,&(marg_s->Acc_pkt_r));
		delay_ms(2); 		
		marg_s->A_x = (marg_s->Acc_buf)[0] | ((marg_s->Acc_buf)[1] << 8);
		marg_s->A_y = (marg_s->Acc_buf)[2] | ((marg_s->Acc_buf)[3] << 8);
		marg_s->A_z = (marg_s->Acc_buf)[4] | ((marg_s->Acc_buf)[5] << 8);
			 
		printf("OUTPUT -METHOD: %d %d %d\n",marg_s->A_x, marg_s->A_y, marg_s->A_z);
	
	
	}	
	
}

/*reads all the sensors, blocking mode*/
void read_sensors(struct marg_sensors_t* marg_s)
{
		
	 switch (marg_s->sensor_t)
	 {
		 case ACCELEROMETER:
		 	 
			 /*read 6 bytes..*/
			 twi_master_read(&AVR32_TWI,&(marg_s->Acc_pkt_r));
			
			 marg_s->A_x = (marg_s->Acc_buf)[0] | ((marg_s->Acc_buf)[1] << 8);
			 marg_s->A_y = (marg_s->Acc_buf)[2] | ((marg_s->Acc_buf)[3] << 8);
			 marg_s->A_z = (marg_s->Acc_buf)[4] | ((marg_s->Acc_buf)[5] << 8);
			 marg_s->A_Set = 1;
			 //printf("ACC");
			  
		 break;
		 case GYROSCOPE:
			  /*read 6 bytes..*/
			  twi_master_read(&AVR32_TWI,&(marg_s->Gyro_pkt_r));
		  
			  marg_s->G_x = (marg_s->Gyro_buf)[0] | ((marg_s->Gyro_buf)[1] << 8);
			  marg_s->G_y = (marg_s->Gyro_buf)[2] | ((marg_s->Gyro_buf)[3] << 8);
			  marg_s->G_z = (marg_s->Gyro_buf)[4] | ((marg_s->Gyro_buf)[5] << 8);
			  marg_s->G_Set = 1;
			  
			  //printf("GYRO");
		 break;
		  case ITG3200:
			  /*read 6 bytes..*/
			  twi_master_read(&AVR32_TWI,&(marg_s->Gyro_pkt_r));
		  
			  marg_s->G_x = (marg_s->Gyro_buf)[1] | ((marg_s->Gyro_buf)[0] << 8);
			  marg_s->G_y = (marg_s->Gyro_buf)[3] | ((marg_s->Gyro_buf)[2] << 8);
			  marg_s->G_z = (marg_s->Gyro_buf)[5] | ((marg_s->Gyro_buf)[4] << 8);
			  marg_s->G_Set = 1;
			  
			  //printf("GYRO");
		 break;
		 case MAGNETOMETER:
		 
			
			  /*read 6 bytes..*/
			  twi_master_read(&AVR32_TWI,&(marg_s->Mag_pkt_r));
		  
			  marg_s->M_x = (marg_s->Mag_buf)[1] | ((marg_s->Mag_buf)[0] << 8);
			  marg_s->M_y = (marg_s->Mag_buf)[3] | ((marg_s->Mag_buf)[2] << 8);
			  marg_s->M_z = (marg_s->Mag_buf)[5] | ((marg_s->Mag_buf)[4] << 8);
			  marg_s->M_Set = 1;
			  //printf("MAG");
			  
		 break;		 
		 default:
		 break;
	 }		 
}

/*early debug function to print all the sensor values with a delay, this function does NOT return
and will block forever*/
/*TODO: use a time for this function*/
void print_serial_sensor(struct marg_sensors_t* marg_s, unsigned int sample_period_ms)
{
	printf("sensor read\n");
	while(true)
	{

		/*probe it to make sure nothing bad happened to the sensor.. if we get rid of it
		there twi_is_busy always return true!!!!*/
		
		marg_s->sensor_t = ACCELEROMETER;
		read_sensors(marg_s);
		
		
		marg_s->sensor_t = ITG3200;
		read_sensors(marg_s);

		
		marg_s->sensor_t = MAGNETOMETER;
		read_sensors(marg_s);
		
		printf("%d %d %d %d %d %d %d %d %d\n",
						marg_s->A_x, marg_s->A_y, marg_s->A_z,
						marg_s->M_x, marg_s->M_y,marg_s->M_z,
						marg_s->G_x, marg_s->G_y, marg_s->G_z);
						
		delay_ms(sample_period_ms);		
	}	
}